/**
 * @file astar_planner.hpp
 * @brief A*路径规划算法的头文件
 * 
 * 该文件定义了基于A*算法的路径规划器，用于在栅格地图上规划从起点到终点的最优路径。
 * A*算法是一种启发式搜索算法，能够高效地找到最短路径。
 */

#ifndef ASTAR_PLANNER_HPP
#define ASTAR_PLANNER_HPP

#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/OccupancyGrid.h>
#include <geometry_msgs/PoseStamped.h>
#include <vector>
#include <queue>
#include <unordered_map>
#include <functional>
#include "auto_navigation/planning/planner_interface.hpp"

namespace auto_navigation {
namespace planning {

/**
 * @class AStarPlanner
 * @brief 基于A*算法的路径规划器类
 * 
 * 该类实现了基于A*算法的路径规划功能，继承自PlannerInterface接口。
 * 通过在栅格地图上使用A*搜索算法，计算从起点到终点的最优路径。
 * 
 * 参数:
 *   - astar/inflation_radius: 障碍物膨胀半径，用于安全距离考虑
 * 
 * 接口:
 *   - initialize(): 初始化规划器
 *   - setMap(): 设置规划使用的栅格地图
 *   - plan(): 执行路径规划，生成从起点到终点的路径
 */
class AStarPlanner : public PlannerInterface {
public:
    /**
     * @brief 构造函数
     */
    AStarPlanner();
    
    /**
     * @brief 析构函数
     */
    virtual ~AStarPlanner();
    
    /**
     * @brief 初始化规划器
     * @param nh ROS节点句柄
     * @return 是否成功初始化
     * 
     * 从参数服务器读取规划器所需的参数，如障碍物膨胀半径等
     */
    virtual bool initialize(ros::NodeHandle& nh) override;
    
    /**
     * @brief 设置地图
     * @param map 占用栅格地图
     * @return 是否成功设置
     * 
     * 设置规划器使用的栅格地图，并提取地图的基本属性
     */
    virtual bool setMap(const nav_msgs::OccupancyGrid& map) override;
    
    /**
     * @brief 规划路径
     * @param start 起始位姿
     * @param goal 目标位姿
     * @param path 输出的路径
     * @return 是否成功规划
     * 
     * 使用A*算法在栅格地图上规划从起点到终点的最优路径
     */
    virtual bool plan(const geometry_msgs::PoseStamped& start,
                     const geometry_msgs::PoseStamped& goal,
                     nav_msgs::Path& path) override;
    
private:
    /**
     * @struct GridCell
     * @brief 栅格单元结构体
     * 
     * 表示栅格地图中的一个单元格，包含x和y坐标
     */
    struct GridCell {
        int x;
        int y;
        
        bool operator==(const GridCell& other) const {
            return x == other.x && y == other.y;
        }
    };
    
    /**
     * @brief 网格单元哈希函数
     */
    struct GridCellHash {
        std::size_t operator()(const GridCell& cell) const {
            return std::hash<int>()(cell.x) ^ (std::hash<int>()(cell.y) << 1);
        }
    };
    
    /**
     * @brief 将世界坐标转换为网格坐标
     * @param wx 世界坐标x
     * @param wy 世界坐标y
     * @param gx 输出的网格坐标x
     * @param gy 输出的网格坐标y
     * @return 是否成功转换
     */
    bool worldToGrid(double wx, double wy, int& gx, int& gy) const;
    
    /**
     * @brief 将网格坐标转换为世界坐标
     * @param gx 网格坐标x
     * @param gy 网格坐标y
     * @param wx 输出的世界坐标x
     * @param wy 输出的世界坐标y
     */
    void gridToWorld(int gx, int gy, double& wx, double& wy) const;
    
    /**
     * @brief 检查网格单元是否有效
     * @param x 网格坐标x
     * @param y 网格坐标y
     * @return 是否有效
     */
    bool isValidCell(int x, int y) const;
    
    /**
     * @brief 检查网格单元是否被占用
     * @param x 网格坐标x
     * @param y 网格坐标y
     * @return 是否被占用
     */
    bool isOccupied(int x, int y) const;
    
    /**
     * @brief 获取网格单元的索引
     * @param x 网格坐标x
     * @param y 网格坐标y
     * @return 索引
     */
    int getIndex(int x, int y) const;
    
    /**
     * @brief 计算两个网格单元之间的启发式距离
     * @param x1 起点网格坐标x
     * @param y1 起点网格坐标y
     * @param x2 终点网格坐标x
     * @param y2 终点网格坐标y
     * @return 启发式距离
     */
    double heuristic(int x1, int y1, int x2, int y2) const;
    
    /**
     * @brief 重建路径
     * @param came_from 路径来源映射表
     * @param current 当前网格单元
     * @param path 输出的路径
     */
    void reconstructPath(const std::unordered_map<GridCell, GridCell, GridCellHash>& came_from,
                         const GridCell& current,
                         nav_msgs::Path& path) const;
    
    nav_msgs::OccupancyGrid map_;  ///< 占用栅格地图
    bool map_received_;            ///< 是否接收到地图
    double inflation_radius_;      ///< 障碍物膨胀半径
    int width_;                    ///< 地图宽度
    int height_;                   ///< 地图高度
    double resolution_;            ///< 地图分辨率
    double origin_x_;              ///< 地图原点x坐标
    double origin_y_;              ///< 地图原点y坐标
};

} // namespace planning
} // namespace auto_navigation

#endif // ASTAR_PLANNER_HPP 